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ATTITUDE REPRESENTATIONS FOR KALMAN FILTERING 

F. Landis Markley* 

The four-component quaternion has the lowest dimensionality possible for a 
globally nonsingular attitude representation, it represents the attitude matrix as a 
homogeneous quadratic function, and its dynamic propagation equation is 
bilinear in the quaternion and the angular velocity. The quaternion is required to 
obey a unit norm constraint, though, so Kalman filters often employ a 
quaternion for the global attitude estimate and a three -component representation 
for small errors about the estimate. We consider these mixed attitude 
representations for both a first-order Extended Kalman filter and a second-order 
filter, as well for quatemion-norm-preserving attitude propagation. 

INTRODUCTION 

The Kalman filter [1] is frequently employed in attitude estimation. A common application is to a 
spacecraft equipped with gyroscopes, with the filter solving for a quaternion parameterizing the attitude and 
a three-vector of gyro drifts [2-5]. Reference [5] discusses this application and provides extensive citations 
of the relevant literature. This paper is an extension of [5], so we begin with a brief review of the 
quaternion attitude representation 

A quaternion is a four-component object with a three-vector pan and a scalar pan [6, 7] 


Quaternions representing spacecraft attitude are generally considered to have unit length; 

kf = |q| : + ?; = i- 

The rotation matrix is a homogeneous quadratic function of the components ot such a quaternion; 

A(q) = (ql - |<jf)/ + 2qq T - 2<7 4 (qx], (3) 

where the 3x3 identity matrix is denoted by I and the cross product matrix is 


0 


Q: ; 

i 

0 

-<7| ! 

l-Q: 

<7i 

0 j 


The quaternion representation is 2—1 because Eq. (3) shows that q and -q represent the same rotation matrix. 
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Since Eq. i 3) and several other equations m spacecraft attitude estimation are nonlinear, an Extended Kalman 
Filter (EKF) is needed (S-10|. The linear quaternion measurement update provided by a straightforward EKF 
does not preserve the nonlinear normalization constraint of Eq. (2). a problem tor which several solutions 
have been proposed [ 5 . 1 1 — 131 - The most direct response is brute force normalization of the quaternion 
following an update. The norm constraint is only violated to second order in a correctly performed update, 
so normalization only changes the update to second order and is therefore outside the purview of the EKF. 
Other approaches have employed pseudo-measurements of the quaternion or of its norm [11. 12 1. 

These modifications do not address an issue that has both conceptual and computational aspects. Unit 
quaternions reside on the three-dimensional sphere S3 embedded in four-dimensional Euclidean space E4. It 
the quaternion errors are small, they lie approximately in the plane tangent to S3 at the true value q of the 
quaternion, which means that they are orthogonal q. This implies that q is an eigenvector of the covariance 
matrix with zero eigenvalue, so the covariance is not positive definite. This is acceptable in principle; but 
numerical errors can lead to loss of positive semidefiniteness and Kalman filter divergence. One approach is 
to relax the requirement of quaternion normalization and parameterize the attitude by 

A -|qj : )/ + 2qq T -2g,[qx]}. (5) 

which is an orthogonal matrix for any q [12]. This avoids both the normalization and zero covariance 
problems; the covariance is nonsingular because the norm of q is not knowm exactly. There is no tirm 
conceptual foundation for a non-normalized quaternion or for pseudo-measurements, though, and 
performance of these methods has not been encouraging. 

In fact there is a deeper conceptual problem with any conventional quaternion Kalman Filter. It would be 
natural to define the quaternion estimate q(r) w^ould be as an expectation value, defined as the integral 

E{< 7 (/)}s p{qj)5(\q\~ -DcTq. (6» 

where p{qj) is the probability distribution function (pdf) on S3. The Dirac delta function enforces the unit 
norm constraint, giving < 5 (|< 7 |~ - l)d 4 ^ as the volume element on S3. Equation ( 6 ) is an unsatisfactory 
definition of < 7 ( 7 ), however, since restricting the probability distribution in quaternion space to the surface 
of a unit sphere means that its expectation value must be inside the sphere. The integral cannot give a unit 
quaternion unless the pdf is concentrated at a point. In fact, since q and -q represent the same attitude, it 
would seem reasonable to take p{qj) = p(-q.t), which gives E{^(r)}-0 lor any pdf. The same issues 
make it difficult to assign a meaning to the covariance of the quaternion error as an integral over S3. 

The widely adopted alternative, which has come to be known as the multiplicative quaternion Kalman filter, 
was developed in 1969 [2, 3], and has been used in NASA programs since 1978 [4, 5]. This uses the four- 
component quaternion representation for the attitude, but a three-component representation for the attitude 
errors. Several justifications have been provided for this non-standard approach to Kalman filtering. The 
approach in Section IX of [5] was to regard the filter as estimating a four-component quaternion, but to 
project the (rank 3) 4 x 4 quaternion covariance and 4x3 quaternion to gyro bias covariance dow n to 3x3 
matrices using a certain 4x3 matrix. However, this formal procedure does not address the conceptual issues 
of defining the quaternion estimate and its covariance as integrals over S3. The second approach, adopted in 
Section XI of [5] and in [13], regards the filter as estimating a three-component attitude error. It must be 
emphasized that these various approaches to the multiplicative Kalman filter lead to identical algorithms. 

The main purpose of this paper is to consider in some depth the relationship between the three-component 
attitude error vector and the quaternion. To set the stage, we begin with a brief discussion of attitude 
representations. We then generalize our results to a second-order filter. Finally, we propose similarly 
motivated techniques to preserve quaternion normalization during attitude propagation. 



VITITUDE P VRAMETER1ZATIONS 


reviews or attitude representations are available [6. 7|. We provide a brief discussion to establish 
conventions and notation for the representations of interest to this paper. 

Rotation Vector 

Euler's Theorem [14] states that the most general motion of a rigid body with one point fixed is a rotation 
by an angle 0 about some axis, which we shall specify by a unit vector e. We often combine the Euler axis 
and angle into a rotation vector $ s 0e. All rotations can be mapped to points inside and on the surface of a 
sphere of radius n in rotation vector space, where points at opposite ends of a diameter represent the same 
180 rotation, It is sometimes convenient to consider a sphere of radius 2n , which provides a 2-1 mapping 
of the rotation group, with each rotation corresponding to a point inside the sphere and a point outside The 
kinematic equation for the rotation vector is transcendental, ill behaved for zero rotation angle, and singular 
lor j 60 3 rotations, so this is not a convenient global attitude representation. A globally nonsingular three- 
dimensional parameterization of the rotation group would be desirable, but this is known to be° 
topologically impossible [15, 16]. 

Rotation Matrix 

The most fundamental representation of the attitude is the rotation matrix 

/?(e,o) = exp[(- 0) x] = (cos 0)1 + (1 -coso)ee T -sin0[ex], (7) 

which obeys the kinematic relation 


R = - [cox]/?(r) . (3) 

where CO is the angular velocity vector. The bilinearity of Eq. (8) in R and (o is very convenient, and the 

skew-symmetry of the cross product matrix preserves the orthogonality of R. Numerical errors can cause the 
direction cosine matrix to lose its orthogonality, though, and orthogonality is not easily restored. This, 
along with its six redundant parameters, makes the rotation matrix inconvenient for numerical work. 

Quaternions 


The unit quaternion is a convenient parameterization of the attitude with only one redundant parameter. It is 
related to the axis and angle of rotation bv 


esin(0 / 2) 
cos(0/2) 


(9) 


The half-angle formulas of trigonometry establish the consistency of Eqs. (3) and (7). The four components 
ot q are the Euler symmetric parameters or the Euler-Rodrigues parameters. They first appeared in paper by 
Euler [17] and in unpublished notes by Gauss [18], but Rodrigues’ classic paper of 1840 first demonstrated 
their general usefulness [19]. Rodrigues was also the first to emphasize the product rule for successive 
rotations, the non-commutativity of rotations, the commutativity of infinitesimal rotations, and the 
generation ot finite rotations from infinitesimal ones. Hamilton introduced the quaternion as an abstract 
mathematical object in 1844 [20], but there is some question as to whether he correctly understood the 
relation between quaternions and rotations [21]. 

We follow Reference [7] in writing the quaternion product as 


P®q = 


r^q^<74P”P x q] 

L P4 7 a - p ■ q j 


( 10 ) 



This differs from the historical multiplication convention, denoted by pq without an infix operator [6, 20|. 
by the sign of the cross product in the vector part. The two products are related by p® q = qp. The 
convention adopted here has the useful property that 

R(p)Riq) = Rip® q). (Hi 


With the historical convention, the quaternion ordering on the right side of the above equation would be the 
reverse of the order on the left side. With either convention, though, the product of two quaternions is 
bilinear in the elements of the component quaternions, a property shared with the direction cosine matrix 
but no other attitude representation. In fact, Eq. (II) means that the rotation group and the quaternion group 
are "almost isomorphic.” The qualifier “almost” is due to the 2-1 nature of the mapping [22]. 


We use an overbar to denote the quaternion representation of a three-vector: 

M 



( 12 ) 


With this convention, the kinematic equation for the quaternion can be written in the alternative forms 


q =4(0 ® q = ^Q(CD )<7 = \Z(q)(0, 
where 4x4 matrix Q(co) and the 4x3 matrix S( q) are defined by [5] 


Q(cn) = 


and 


(13) 
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The skew-symmetry of Q((0) preserves the normalization of q , but this normalization may be lost due to 
computational errors. If so, it can be restored trivially by q - <7/M» which is a much simpler operation than 
orthogonalizing an approximately orthogonal direction cosine matrix. 


Gibbs Vector or Rodrigues Parameters 

The three Rodrigues parameters are defined by [6, 7, 19] 

q esin(0/2) 
® q x cos(>/2) 


etan(o/2). 


( 16 ) 


This has the inverse relation 


<7 = 



g 

1 ’ 


(17) 



w ere wo use an italic letter to represent the magnitude of a three-vector, except when the latter is the vector 
part of a quaternion. These parameters also give the Cayley parameterization [231, and Gibbs arrayed them 
in a vector semitangent ot version” [24|. [t is little wonder that we now call it the Gibbs vector. The 
Gibbs vector is a gnomonic projecton. a 2-1 mapping of the S3 quaternion space onto three-dimensional 
buclidean g space, with <7 and -q mapping to the same point, as shown in Fig. 1 . Since q and -q represent 
the same rotation, the Gibbs vector parameterization is a l-I representation of the rotations. The Gibbs 
vector is infinite tor 130° rotations (the q t = 0 equator of S3), which is undesirable for a 2 lobal 
representation of rotations. 

Modified Rodrigues Parameters 

These parameters, defined by [ 7 j 


P = 


q 

[ + q. 


esin(< 2 )/ 2 ) 

T — r- = etan(£>/4). 

i + cos( 0 / 2 ) 


were introduced by Wiener [25] and rediscovered by Marandi and Modi [26], who established their 
interpretation as a stereographic projection. The inverse relation is 


(18) 


2p 


e relation between the quaternion and the modified Rodrigues parameters is like a stereographic projection 
ot S j quaternion space, as shown in Fig. 2. One hemisphere of S3 projects to interior of the unit sphere in 
three-dimensional p space, and the other hemisphere of S3 projects to the exterior of the unit p-sphere. 

All rotations can be represented by modified Rodrigues parameters inside and on the surface of the unit ball 
The two parameters p and - p/ p- represent the same rotation. Thus the origin and the point at infinity 
both represent a zero rotation. The properties of the modified Rodrigues parameters are very similar to those 
o. the rotation vector, but no trigonometric functions are needed to express the rotation matrix in terms of 
the former parameters. 




Figure I. Gibbs Vector 
as a Gnomonic Projection 


Figure 2 . Modified Rodrigues Parameters 
as a Stereographic Projection 


EXTENDED KALMAN FILTER 

The multiplicative EKF represents the attitude quaternion by 

qU)=&iiA(t))®q rrf (t\. (20) 


where q rel (n is some normalized reference quaternion and <5fji(a(f)) is a unit quaternion parameterizing the 
rotation from the reference attitude parameterized by q„,(.t ) to the true attitude parameterized by 17(f). We 
choose the reference quaternion so that Sq is close to the identity quaternion and can be represented by a 
three-dimensional parameterization, which we denote by a for attitude. The two attitude representations a(f) 
and q rrr if) in Eq. (20) are clearly redundant. We choose this form so that the three-component a(r) can keep 
the statistics straight while assuring exact quaternion normalization, and the four-component q, tf (t) can 
provide a globally nonsingular attitude representation. 


One possible parameterization of a is the rotation vector <p e, which we denote a,, , so from Eq. (9), 


&7 = 


(a,/a,)sin(a, 12) 
cos(a, 12) 


(21a) 


We allow a, to range over a ball of radius /rto cover the entire rotation group, but expect it to be close to 
the origin if <7 .is close to q. This parameterization has the advantage that the covariance will include the 
angular variances in radians", but it is numerically inconvenient. A special form, such as a Taylor series, 
must be used near a, = 0. We can retain the interpretation of the covariance matrix in the small angle 
approximation by requiring a to have the same first-order limit as a function of the rotation vector. This 
leads to the second parameterization of a as twice the vector part of Sq, which is the parameterization used 
in Section XI of [5], except for the factor of two: 


Sq = 



(21b) 


where a 7 ranges over a ball of radius 2. A third parameterization is twice the Gibbs vector, from Eq. (17), 


Sq = 



(21c) 


where a ? ranstes over all of E3. A fourth alternative is four times the vector of modified Rodrigues 
parameters, so from Eq. (19), 


Sq = 


1 

16 + a; 


8a, 

16 -a;J’ 


(21d) 


where a, ranges over a ball of radius 4. This parameterization has the computational advantage of not 
requiring any transcendental functions. These four definitions of a have the same second-order 
approximation. 


Sq = 


a/2 

1 -a : /8_ 


( 22 ) 


Thus they are equivalent for an EKF, which uses a linear approximation; but they differ in third and higher 
orders in a. It is worthwhile to note that Eq. (22) only holds to first order if the components of a are taken 
to be Euler angle rotations about three orthogonal axes, as in (2. 3]. Thus, an Euler angle parameterization 
will lead to the same EKF. but will give different results for a second order filter. 



Liquations i ZOt and (2 1 ) map a quaternion pdf from a hemisphere of S3 with q rrt u) at its pole onto a pdf of 
a over the appropriate subset of E3 as defined above for the particular parameterization chosen. There is no 
loss ot information in mapping only one hemisphere of S3, since the other hemisphere contains the same 
information due to the 2-1 nature of the quaternion representation and the assumed symmetry 
pii/.n = p( -q.t). We then compute a(r), the expectation value of a, in the conventional way as the 
integral of the product ot a and its pdf. This allows us to define the best estimate of the quaternion by 

qU)sSq(a(t))®q rr/ ([), (23) 

which is a unit quaternion, and thus an acceptable estimate of the attitude. It appears that we are free to 
choose the reference quaternion q Kf (t) at our convenience, but this choice is not arbitrary, since the value 
ot q(t) depends on the choice of q, tf U). The examples in Appendix A show that the best estimate is 
obtained by choosing the reference quaternion q n/ (t) so that the expectation value a (r) is identically zero 
W.th this choice, which we adopt in this paper. Eq. (23) shows that q, r/ (i) is identically equal to q(t). so 
we will denote it as such. This means in turn that 5q(a(t)) is a representation of the attitude error. This 
choice has the additional computational advantage that it obviates the need to propagate a(r). 

State Propagation 


The filter dynamics are found by differentiating 


inserting Eq. (13) and the identity 


which gives 


& 7 (r) = q(t)® q~ l (t), 
d< 7 ~'/dr = -q~ l ®q®q~ l . 


d(Sq)/dt ~\(i>®Sq-Sq®q®q~ l . 


(24) 

(25) 

(26) 


Now let us consider the Gibbs vector parameterization for specificity. This parameterization has the 
advantage that it takes values over all of E3 rather than only over a finite subset. Solving Eqs. (20) and 
(21c) for a. gives 

V f) =2(Sq) v /(5q) i . ( 27 ) 

Equations (2 1c). (26) and (27) give 


a f (r) = Uco(r)® 


V f) 


a s (0 

9 


®<?(r)®<7' l (r) 




a,(0 


(28) 


® q(t)® < 7 “‘(m a (r) = f(x(r),r). 


Now we specialize to the case of attitude and gyro drift estimation [5J. In this case, the angular rate vector is 
given in terms of the gyro output vector u(r) and gyro drift vector b(r) in spacecraft body coordinates by 


(a (r) = u(r) - b(r) - r|, (r) , ( 29 ) 

where r|,(r) is a zero-mean white noise process. The gyro drift vector obeys 


b(r) = T),(r) . 

where r|.(r) is an independent zero-mean white noise process. 


( 30 ) 



Wo want to construct an extended Kalman ;;ne; n>r die six-component ^tate vector 


xu) 


au) j 

bu) j 


131 } 


where we suppress the subscript on a in equations that hold for any three-dimensional parameterization. 
This state vector obeys the propagation equation 


XU) 


[f (x(r),r); 

L ifc(r) J' 


(32) 


where the time dependence of the reference quaternion is implicitly included in the time argument of 
ff x(r)u). The expectation value of this equation propagates the state estimate in the absence of 
measurements. In the usual linear EKF approximation, which is to ignore correlations in computing the 
expectation value, 


a 



b 


0 


(33) 


We want to choose q{t) so that a it) is identically zero, and the expectation value of Eqs. (28) and (29) 
with a(r) and a(r) equal to zero gives 

{q(t)®q~ l (t)} v = -V[u(f) - b(r)j = -rm(r). (34) 

The normalization of q{t) requires 


[q{t)®q _1 (r)} 4 = 0 . (35) 

so 

q(t) =tCO(f)® q(t) =±Q.{ti>(t))q(t) = *S(<7(0)©U), (36) 

Appendix B shows that Eq. (36) does not depend on our choice of three-dimensional parameterizations of the 
attitude. This quaternion propagation equation is the same as the equation derived by more conventional 
methods, but we have derived it from the requirement that a(r) be identically zero. The usual approach must 
either postulate Eq. (36) a priori or derive it from dubious arguments using Eq. (6). 

Measurement Model and Covariance Propagation 

Processing a set of measurements at a discrete time t k in a standard Kalman filter for the state vector defined 
in Eq. (31) produces a post-update estimate x(r t +) in which the three-component attitude estimate a(/ 4 +) 
will not be zero, in general. In order to avoid the need to propagate two different representations of the 
attitude, the attitude information is moved from a(r 4 +) to q(t k +) . after which a(r 4 -*-) is set to zero. Since 
the value of the true quaternion is not changed by these operations, Eq. (20) requires 

<5*?(a(/<+))® <j(r 4 -) =<57(0)® q(t k +)- (37) 


Since Sq( 0) is the identity quaternion, this gives 

<?(f 4 +) = 57(a(/ t +))® qU k -)- 

This procedure of moving the update information from af/ 4 + ) to q(t k -r ) . is called a reset. 


( 38 ) 



The reset ‘-pe.at.on is assumed not to modify the covariance. This seems logical, since the total 
information content of the estimate is neither increased nor decreased by the reset; it is merely moved form 
one part ot the attitude representation to another. The discrete reset operation is a cause for some concern 
though, so we want to hnd some means to avoid it. 

We can eliminate the discrete reset operation by keeping a(r) s 0 at all times, even during the update We 
accomplish this by considering each attitude measurement update to be spread out over an infinitesimal time 
interval, rather than being instantaneous. Thus the *th attitude measurement update is assumed to take place 
over the infinitesimal time interval from r t to t k + e . Since the measurements are actually discrete and not 
truly continuous, we are really only interested in the limit that e -» 0. In the reset termmolo°v. the filter I 
am presenting here is a Continuous Reset Extended Kalman Filter. It is difficult to resist the temptation to 
call it the CoREK Filter, to contrast it with the other (inCoREK) filters. 

For non-instantaneous measurements, Eq. (33) is replaced by (8— 10J 


x(/) = 


f(x(/),r) 

0 


+ P{t)H T (j)R~\t)[z(t) - h(x(r).r)]. 


(39) 


where z(r) is an m-dimensional vector of attitude measurements and h(x(r),r) is an m-dimensional vector of 
measurement models with the time dependence of the reference quaternion q(t ) implicitly included in its 
time argument. The covariance can be partitioned into 3x3 submatrices as 

>,(0 P c ( t )] 

p„<t) 


P(t) a £{(x -x)(x -x) r ) = 
and the m x 6 measurement sensitivity matrix is 


(40) 


H = 


"3h 



_3a 

3b j 



0 . 
3a 




since the attitude measurements are assumed not to depend explicitly on the gyro drifts. This gives 
PU)H r {t)R- l it)[z(t) - ht.x(r).r)] = 


E,(r)(3h/3a) 7 '/r l (r)[z<r) - h(x(r),r)] ' 


[”o(x(r).r)" 

P/(r)(ah/3a) r /?- l (r)[z(r)-h(x(r),r)]j 


_P(x(r).r) j 


(41) 


(42) 



The propagation of the Gibbs-vector-based representation including measurements is 

0<?(r)0^ _1 (/)[ a f (/) + a(x(r),r), 

J 4 

Equation (43) with a { (r) and a f (r) equal to zero gives 

[q(r)®q‘ l {t)} v = +[©(/) + a(x(/),r)]. 

so, with Eq. (35). 

q{t) = +[(b(r) + a(x(r).r)]«S^(r) = +Q(db(r)-t-a(x(r)./))^r) = 4-H(^r))[d)(/) + a(x(/).r)]. 


(43) 


(44) 


( 45 ) 



,V> before, these results hold tor j general three-dimensional attitude representation obeying Eq. (22). 
Equation (45) and the drift bias equation 

b = P<x(r).r) = Pfidh/da/ R~'(!)[un -h<x(f).n|. (46) 


are the estimate propagation equations including measurements, since we have chosen the reference 
quaternion so that a(r) is identically zero. 

Substituting Eq. (45) into Eq. (28) gives, after some algebra. 

f(x(r).r) = -cb(/) x a f (r) + Ato(r) - aix(r).r) — +(Aa>(r ) + a(xU).r)] x a ? (r) 

+ 4-([Ao)(r)-a(x(0,f)]'a { (r)}a ? (r), 

where 

A co(0 s c o(f) - (b(0 = -b (r) r b(r) - ii, (r) . 


( 47 ) 


(48) 


It follows that the covariance matrix obeys 

P(t) = F(t)P(t)+P{t)F T {t) + G{t)Q{t)G T (t)-P{t)H T {t)R-'(t)H{t)P{t ), (49) 


where 


and 


F(t) = 


GO) 


E{df/3a} E {3f/db} 


-{(O(r)x] -/*, 3 


L 0 3x3 

0,3 J l 


0jx3 

0 jx 3 

jEfdf/aii,} 

E {df/3T| 2 } 


^3x3 ^3x3 

L 0 3*3 

f 3x3 


_° 3 x 3 

f 3x3 


Q0) = 


0,(0 0,3 

03,3 ClW 


E{»l,WTly(0} =<5 v 5(r-r')(2,W for i.j= 1. 2 . 


(50) 


(51) 

(52) 


(53) 


Recalling that the attitude measurements are discrete and not truly continuous, we see that the covariance 
propagation during the intervals between the attitude measurement updates, when the last term in Eq. (49) 
is absent, is identical to [2-5], except for factors of 4- in some of the formulations. 


Covariance Update 

In considering the kth measurement update, we will treat the mx 3 measurement noise matrix R(t) and 
m x m measurement sensitivity matrix Hit) as constants during the infinitesimal interval: 


and 



(54) 


R(t) = £/?*, 


(55) 


where R k is the equivalent discrete measurement noise matrix as on p. 122 of [8]. The appearance of £ in 
the measurement noise matrix is the reason we get a finite measurement update to the state and covariance 
in the e — > 0 limit. We can ignore all the non-measurement terms in the equations for the state estimates 



•: nd thC C0var,ancc durm = lhc “'«** ■*'» a-ve corrections of that go to zero m th.s l,m,t. The 

covariance propagation equation during the update is 


Pit) = - Pit) 


f H t r (£R k r , H i o.. ; ] 


0.. 


0 . . 

J 


Pin 


(56) 


The solution of this is simply 


p U) = p-'u^+i 


! 0 ; ' r 


b'sing the matrix inversion lemma [9] gives 


P(') = P(t k )-P(t k ) 


Hi 




o s „ 


H.PMH [«. 


(57) 


t-t, 


(58) 


which reduces to the usual form at t k + = t k + £ , the end of the finite-time update, 

'Hi 


PU i +)=PU k )-P(t k ) I 


3x/n J 


[H k Pjt k )Hl + P t J~'[H k 0 mx3 ]P(t k ). 


(59) 


Thts update is also identical to the standard update [2-5], except for occasional factors of +. 

Measurement Update 

estimation equation during the update, again ignoring terms that go to zero in the e -> 0 
q(t) = \E(q(t))a(q(t).t) = 4-E(<7(r)) P a (t)Hl (e R k )-'[z(r) - h(^(r),r)] 


= +H(^(r»j PJt k )~ P a (t k )Hl I H k P J (t i )Hl + £ R> 


v k_ 


\ > 

H.PJtAnl (£ R t r‘[z(t) - h (q(t).t)] 


J 


I 

= iZ(q(t))P a (t k )H[\/- 


H k P«(t k )Hl + £Ri 


t-t ■ 


H t P a (t k )H T k }(£ R k )-'[ Z (r) - h(< 7 (/), r)] 


(60) 


i-(4(f ()P a ( [ k)Hl[(t - t k )H k P a (t k )H k + £/? t ] [z(r) - h(^(r),r)]. 


Several equations have been used to obtain this result, Eqs. (42), (45), and (58) in particular. Taking the 
measurement to be constant, z (/) = z t , and assuming that the only time dependence of h(q(t),t) = h t (<?(/)) 
is through the quaternion, the measurement residual time dependence can be computed by 


~M<7(r))) = - - h ,* (?) 
dr dq 


q(t). 


(61) 


!(/ = //( r> 


We evaluate this partial derivative for the Gibbs vector parameterization of a. Equation (5) can be written 


a ( f) = = UlMiM 

[q(t)®q-\t)] k q T U)q 


( 62 ) 



( 63 ) 


Then, using the chain rule t'or differentiation and the identity 

Z r (q)q = 0. 


we find that 


dh k (q) 

dh* 


dq 

ry=iy S 

dq 

a=0 


= 2 H k E r (q(t)). 


It is shown in Appendix B that this result is independent of our choice of three-dimensional 
parameterization. Inserting Eqs. (60) and (64) into Eq. (61) gives 


_d 
d / 


z t - h k (q(t))] = -H k P a (t k )H T k [(r - t k )H k P u (t k )H T k + £ R k 


[z* -h 4 (<?(r))]. 


where we have used the identity 


Z T (q)Z{q) = I ixi . 


The solution of Eq. (64) is 

[** -h k {qm = SR k \{t-t.)H k P J {t k )Hl +ER k }"[z k -h k (q(t k ))]. 


(64) 


(65) 


( 66 ) 

(67) 


The validity of this solution can be verified by differentiating and using the identity 

H k Pjt k )Hl[(t-t k )H k P a (t k )H; + e R t {' e R k = e R k [{t -t k )H k P a (t k )Hl + £ R k }'' H k P a (t k )Hl ( 68 ) 
This identity is clearly true if t = t k . If t * t k , its truth is demonstrated by 

(r - t k )H k PJt k )Hl [(r - t k )H k P a (t k )H T k + £ R k ]"' £ R k 

= \j-eR k [u-t k )H k P a (t k )H T k +£« 4 ]"}e* t =eR k [l-[{t-t k )H k P a {t k )H T k + £/? t ]' l £tf t } (69) 

= £ R k [<r - t k )H k P, (t k )H k + e R k ]"'(/- t k )H k P d ( t k )H T k . 

Inserting Eq. (6 7 ) into Eq. (60) gives 

^(r) = 4-E(<7(r))P fl (r t )x(f) = \Q(P d (t k )xU))q(t). (70) 

where 

X(r) = H T k [(t-t k )H k P a (t k )H T k +£R k }"' eR k [(t -t k )H k P a (t k )H T k +eR k }' l [z k -h k (q(t k ))] 

dr ■, (71) 

= -((t-r*)«[ ((r (r t )W[ +£R t ] [z t -h t (<7(r t ))]j. 

The solution of this equation in the £ — > 0 limit is [6] 

q(t k +) = zxp(\Q($ k ))qU k ) . (72) 

where 

* k = PJ' k )j'''' X(‘)dt = P a (t k )H T k [H k P„(t k )H T k + R k ]‘‘[z t -h t ( 9 (r t ))]. (73) 

Similar computations starting with Eq. (46) give the time dependence of the bias as 


b(r) = P r (f*)X('). 


(74) 


which has the solution 


bu^j = b(r, + -R t \-\ Zk -h t (<}(, t ))l. (75) 

^ dntt bKlS u ? date ls the itMdard EKF result [2-5]; but the quaternton update differs in preserving the 

r.nTa'TsSi^ T C r y , ^ ^ $t WOM ** * ( '‘ +) m " conventlo ^l EKF with a discrete reset 
. q ' ' e treatment presented here. appears as a rotation vector independent of which three 
d menstona! parametenzat.on we choose to represent the attitude error. The conventional proceed Is more 
efhaent than the norm-preserving update derived here, however, espec.aliy as .mplementtSd in [4]- and these 
methods are equivalent to second order in the measurement residuals. 

SECOND ORDER FILTER 

second order »** **» erection terms obtains the essential benefit of a 

second order filter without the computational penalty of additional second moment calculations. This filter 

adds second-order corrections to the state propagation and measurement residual equations, but uses the EKF 
expressions for the covariance and gains. In the continuous measurement case. Eq (39) is replaced by 


- (/) J f( x(r).r) + b p (r) 




0 


where 


and 


b M) s itr 


r P(t)H T {t)R~\t)[z(t) - h(x(r),r) - b m (r)] , 

d’/ t (x,r) 


3x 2 


■P{t) 




b^(t) s+tr 


fd% Ou) 

3x 2 


P(0 


= -rtr 


3 2 /i t (a,r) 

3a 2 


PM) 


(76) 


(77) 


(78) 


!a=i(r) 


<7SI re “' B fr ° m ‘ hc ' aCk of “P' idt bi “ “«!«»*="» of .he measurement. In 
parallel with Eq. (42), we write the measurement part of Eq. (76) as 


P(r)// r (r)/?~'’(r)[z(f) -h(x(r),r) - b m (r)] = 


P a (r)(dh/9a) r /T l (r)[ z(r) - h(x(r),r) - b m (r)] 
fj(t)(dh/da) r R- l (t)[z(t) - h(x(r),r) - b ra (r)] 
a'(x(r),t) 


(79) 


_0'(x(r),r)J‘ 

For the Gibbs vector parameterization, Eqs. (28), (40) and (77) give 

b P (0 =|F J [6(r)-2[^(r)®^ 1 (r)] v } + 0 )^^). 

where a),, has components 

5 for i = 1. 2. 3. 
wuh e t/t being the totally antisymmetric Levi-Civita density: 

~ £ :j| = e 3i; = I. 

£ j:i = e iu = e i3: = -1 • 

£ t,k = 0 >f any two indices are equal. 


(80) 


(81) 


(82a) 

(82b) 

(82c) 


Including these second order terms into Eq '4.5) gives 


a <r) = J r<o(r ) 0 


a ( in : 1 a { tn 


0 r/U)0 </ ( r ) | 


- -r Vdiu ) 0 


a t U) 


a f ( t ) 


0 < 717 ) 0 17 ' ; (f)f a ( tr) 


- t PJcotr) - 2[ q(t) 0 <7 1 (r)] v ) - a},.(n - a'(xU).t) . 
The condition that a ? (r) and a t (r) are equal to zero is 

[<7(r)0 4 ''(/)I v . = ^{to(r)-^/ U)]'^,(n + a' (x(fU)I}. 


( 83 ) 


(84) 


It is shown in Appendix B that the factor of [/ + 1 PJOf 1 depends on the specific choice of the three- ? 
dimensional parameterization of the rotation. Since P„(t) is second order in the estimation errors and this 
factor multiplies terms of first order in errors, it is consistent with a second-order filter to ignore it, giving 

q{t) = ^[<a(r) + a> c (r) + a'(x(r),r))]0 <?(f) =-VH(g(r))[to(r) + a> c (r) + a'(x(r),r)]. (85) 


The time dependence of the gyro drift bias is given by 

b = 3'(x(r),r). (86) 

For time propagation between measurements, the Ct and p terms are zero, so the only change from the EKF 
is the addition of the term involving 0) c (r) in the quaternion propagation. This is a second-order correction 
to the angular rate vector arising from the skew part of the covariance between the attitude errors and gyro 
drift bias errors. The measurement update equations in the £ — > 0 limit are given by 

<7(r.4-) =exp(-VQ($'))^(fi). (37) 

where 

V = PJt<) H l[ H < p J t ^H T k +p k ]" l (z i -MtfM-Mr*)]. (88) 

and 

b(r t +) = b (t k ) + P c r (t k )Hl[H k PJt k )Hl +R k ]'‘[z* -h t (?(r t » -b^r*)]. (89) 

Except for the maintenance of quaternion normalization by Eq. (87), these results are equivalent to those 
obtained by Vathsal [27]. It should be pointed out that Vathsal also considered second order effects on the 
covariance and gain matrices. The derivation of Eqs. ( 87) — (89) assumes that b m (f) is constant over the 
infinitesimal duration £of the update; this approximation needs to be verified for a specific measurement 
model. 


QUATERNION INTEGRATION 

Exact propagation of the quaternion using Eq. ( 13) would preserve the quaternion norm exactly, due to the 
antisymmetry of the matrix Q(co). The simplicity of this propagation, especially its linearity in q , is often 
ajven as one of the advantages of the quaternion representation of the attitude. However, the quaternion 
norm is not preserved exactly in many numerical integration procedures, for example in commonly 
employed Runge-Kutta integrators. We propose a new solution to this problem and contrast it with several 
alternative strategies for enforcing quaternion normalization. The criterion for comparing these methods 
should be the attitude error rather than the quaternion normalization error, since the latter contains no 
attitude information. 



New Method 


Th.s method is based on the ideas about attitude representations presented in this paper It uses a three- 
jomponent representation of the attitude variation over an integration step, and the quaternion representation 
tor the accumulated attitude. This gives up the advantage of the bilinear quaternion differential equation to 
preserve the quaternion norm. One alternative is to integrate the Gibbs vector kinematic equation [7] 

g = -Vfoj -Q)xg + f(o g)g|. (90) 

from r . to with the initial value g, = 0. At the end of the integration step, the quaternion is updated bv 


<7^, =<7(g„.,>®<7„ (91, 

where < 7 (g„ w ) is given by Eq. (17). The square root in this equation can be avoided by using the Modified 
Rodriges Parameters, whose kinematic equation is [7] 


P = i [( 1 - p 1 )co - 2 to x p + 2(co • p)p] . 

This is integrated from t„ to with the initial value p„ = 0. and then the quaternion is updated by 


(92) 




yyj) 


where q( p i+1 ) is given by Eq. (19). 

Direct Integration 

The conventional approach is straightforward integration of Eq. (13). Since quaternion normalization is not 
automatically preserved, we normalize the quaternion by setting q = q/^. We can normalize the quaternion 
either once after the completion of the entire range of integration or after each integration step. 

Closed-Form Solution 

This method is based explicitly on the linearity of Eq. (13). It gives the solution as a matrix exponential [6] 

= ex P(i ^( < M)<7* = /cos(<z>„/2) + <?; 1 )sin(c> n /2), ( 94 ) 


where 


4>i = J'" a>(r)dr. 


(95) 


This solution is only approximate unless the direction of © is constant, which is why Eq. (72) only holds 

in the in the e -4 0 limit. Equation (95) gives the lowest order of a Magnus expansion [28], which is 
related to the Baker-Campbell-Hausdorff formula. The next higher approximation is 


=/ f <o(f)dr + (/i : /l2)©(r,)x©(r (iH ), 


(96) 


where h - r^, -t n is the integration step size. 

Constraint-Preserving Integrator 

A recently proposed a second-order method assuring exact quaternion norm preservation is [29, 30] 

i = [ 1 + (*/■*)■ <u*^] [/ + q n • 

Higher order methods have been proposed [30], but these require higher derivatives of the angular rates, 
which are not generally available without additional computations. 


SUMMARY 

The major result of this paper is to clarity the relationship between the tour-component quaternion 
representation of attitude and the three-component representation of attitude errors in the widely used 
extended Kalman filter that has become known as the multiplicative Kalman filter. We view this filter as 
based on an apparently redundant representation of the attitude in terms of a reference quaternion and a three- 
vector specifying the deviation of the attitude from the reference. This apparent redundancy is removed by 
constraining the reference quaternion so that the expectation value of the three-vector of attitude deviations 
is identically zero. It is therefore not necessary to compute this identically zero expectation value. The basic 
structure of the multiplicative Kalman filter follows trom constraining the reference quaternion in this 
fashion: the reference quaternion becomes the attitude estimate, the three-vector becomes the attitude error 
vector, and the covariance of the three-vector becomes the attitude covariance. All these results are well 
known in practice, but the justification for using this mixed attitude representation has been unclear. 

An explicitly norm-preserving measurement update of the quaternion has been developed in this paper. It is 
less efficient computationally than the conventional update, however, and it is mathematically equivalent 
through second order terms in the measurement residuals. We have also investigated a second-order filter in 
the new framework, but have merely reproduced results obtained previously. 

The idea of representing the attitude in terms of a reference quaternion and a three- vector specifying the 
deviation of the attitude from the reference can also be used to develop norm-preserving quaternion 
integrators. Some algorithms in this area are proposed. 

APPENDIX A 

We’ll consider two toy problems to illustrate the assertions about pdfs below Eq. (23). In each case, we II 
start with a pdf on S3 that obeys p(qj) = p(-?,r)and has two well-defined maxima. We would like our 
estimate q to have one of these values. We denote the reference quaternion by q , since we won t assume 
that a = 0 in this Appendix. 


Case 1 

Consider a pdf concentrated at the poles 



of S3: 


p(q ) = d 31 (q) ’ 


(Al) 


Mapping this to p(a f ) using Eq. (2ic) gives 


p(a ) =<5 


'( 3 ) 


/ 

<7,„4 a, + 2 q„ 

/ 

\ 

*, x 



a J 

■ 2 

1 ) 


(A2) 


If q * 0. it's clear that a { must be such that the argument of the delta function tn Eq. (A2) is zero. This 

2< W 


w 

ojves 


<lr ff X 


Then Eq. (23) gives 


q = sign{q rtfi ) 


(•A3) 


(A4) 


the desired result. If q„ fi = 0, then a, = 0 by symmetry, and q = q, rf . 



Case 2 


Consider a pdf with a very broad distribution centered at the poles ± 

4 </;U 


i 


of S3: 


P (q) = 


<A5j 


Mapping this to p(a f ) using Eq. (2 Ic) gives 


g( , t ,. 4 . ^ 


/T 


4 + a 


(A6) 


The expectation value of a,, using the appropriate volume element, is 


* f 2d 3 a 

a <=J a rPK) 


4 + a. 


8 f a ,(2^/4-a f q„,) 2 
~ rr J 7 ^ d a * = " 4 

1 4+ WJ 


^q 


i«/’ 


(A7) 


after a tedious but straightforward integration. Then Eq. (23) gives 

(i-2^ /4 )q, r/ 


1 + 4 <7^/4 q„/ 


(AS) 


The special case of q rtfi - ±1 gives the desired result of Eq. (A4), but this is not true in general. It is worth 
noting, though, that Eq. (AS) gives q\ > 0.95 (where the pdf is greater than 95% of its maximum value) 
for k 4 |> 0.562. 

APPENDIX B 

Instead of using the Gibbs vector, we will employ the vector part of the quaternion as in Eq ('’lb) With 
this parameterization, Eq. (43) is replaced by 


a,(') = |+ttK0® 


a,( t) 


J 




®q(t)®q "'(/)[ +a (x(r),r). 


(B2) 


Solving for a q (t) and k q {t) equal to zero gives Eq. (44). Omitting the measurement term gives Eq. (34). 
Substituting Eq. (45) into the right side of Eq. (B2) gives, after some algebra, 

f(.x(r),r) = -to(r)x a,,(r) + 4^4 - a 2 (r)[Ao)(r) -a(x(/),r)] - l[Ao>(f) + a(x(r).r)]x a q (t). (B3) 
It follows that Eqs. (49)— (52) for the EKE are unchanged. Differentiating Eq. (B2) gives, with Eq. (77), 

b p (t) = -j trP,(r) (d)(r) - 2[qU) ® q '‘(r)U + G> c (r) , (B4) 

in place of Eq. (80), which changes Eq. (84) to 

( 17 (f)® < 7 'V)]„ = +(o>(f) + [l -itrP 1 (f)]' l (aj i .(f) + a'(x(f),f)]}. ( 35 ) 

Equations (84) and (B5) agree if and only if we ignore products of PJt) and terms of first order in the errors. 


This parameterization of a gives 


a ft) = 2[</u)®<7*'(r)| v = 2 H r (r/(r))</. (B6) 

so evaluating the measurement partial derivative using the chain rule leads directly to Eq (64). 

REFERENCES 

1 . Kalman, R. E„ "A New Approach to Linear Filtering and Prediction Problems." Transactions of the 
ASME, Series D, Journal of Basic Engineering, Vol. 82, pp. 35 — 15, 1960 

2. Paulson, D. C., Jackson, D. B., and Brown, C. D., “SPARS Algorithms and Simulation Results.” 
Proceedings of the Symposium on Spacecraft Attitude Determination, Aerospace Corp. Report TR- 
0066 (5306)- 12, Vol. 1, pp. 293-317, Sept.-Oct. 1969. 

3. Toda, N. F„ J. L. Heiss, and F. H. Schlee. “SPARS: the System. Algorithm, and Test Results,” ■ 
Proceedings of the Symposium on Spacecraft Attitude Determination , Aerospace Corp. Report TR- 
0066 (5306)- 12, Vol. 1, pp. 361-370, Sept.-Oct. 1969 

4. Murrell, James W„ “Precision Attitude Determination for Multimission Spacecraft,” AJAA Paper 78- 
1248, AIAA Guidance and Control Conference, Palo Alto, CA, August 1978 

5. Lefferts, E. J., F. L. Markley, and M. D. Shuster, “Kalman Filtering for Spacecraft Attitude 
Estimation,” Journal of Guidance, Control, and Dynamics, Vol. 5, No. 5, pp. 417 — 129, 1982 

6. Wertz, James R., ed.. Spacecraft Attitude Determination and Control, Dordrecht, Holland, D. Reidel, 
1978 

7. Shuster, Malcolm D., “A Survey of Attitude Representations,” Journal of the Astronautical Sciences, 
Vol. 41. No. 4. pp. 439-517. 1993 

8. Gelb, .Arthur, ed.. Applied Optimal Estimation, Cambridge, MA, the MIT Press, 1974 

9. Stengel, Robert F.. Optimal Control and Estimation, New York. Dover Publications, Inc., 1994 

10. Maybeck, Peter S.. Stochastic Models, Estimation, and Control, Arlington, V A, Navtech Book and 
Software Store, 1994, see especially Volume 2, pp. 224—225 

11. Bar-Itzhack, I. Y., J. Deutschmann, and F. L. Markley, “Quaternion Normalization in Additive EKF 
for Spacecraft Attitude Determination,” AIAA Paper 91-2706. AIAA Guidance, Navigation, and 
Control Conference, New Orleans, LA, August 1991 

12. Deutschmann. J., F. L. Markley, and I. Bar-Itzhack, “Quaternion Normalization in Spacecraft Attitude 
Determination,” Flight Mechanics/Estimation Theory Symposium, NASA Conference Publication 
3186, Goddard Space Flight Center, Greenbelt, MD, May 1992 

13. Shuster, Malcolm D., "The Quaternion in the Kalman Filter." Advancer in the Astronautical Sciences. 
Vol. 85. pp. 25-37, 1993 

14. Euler, L.. “Formulae Generales pro Trandlatione Quacunque Corporum Rigidorum,” Novi Comment. 
Petrop., Vol. 20, pp. 189-207, 1775 

15. Stuelpnagel, John, “On the Parameterization of the Three-Dimensional Rotation Group," SIAM 
Review, Vol. 6. No. 4, pp. 422-430, 1964 

16. Hamermesh, Morton, Group Theory and its Application to Physical Problems, Reading, MA. Addison- 
Wesley, 1962, Section 9-2 



l? Euler. L . "Problema Algebraicum Ob Affectiones Prorsus Singulares Memorabite." Novi Comment 
Petrop . Vol. 15, Section 33. p. 101. 1770 

13 PwT C F Werke ' Vo1 ' Vin ' PP ' 357 ~ 362 - Gottingen. Kdnigliche Gesellschaft der Wisssenscaften. 

19 Rodrigues, O.. “Des lois geometriques qui regissent les deplacements d'un systeme solide dans 1’espace, 
et de la variation des coordonnees provenant de ces deplacements considers independamment des causes 
qui peuvent les produire,” Journal de Mathematiques , Vol. 5, pp. 380-440, 1840 

20. Hamilton, W R.. “On Quaternions; or a New System of Imaginaries in Algebra." Philosophical 
Magazine , 3 Series, Vol. 25, pp. 489— 495. 1844 

21. Altmann, Simon L. t “Hamilton, Rodrigues, and the Quaternion Scandal.” Mathematics Maea-ine Vol 
62, No. 5, pp. 291-308, 1989 

22. Curtis, Morton L., Matrix Groups , 2“ J ed.. New York, NY, Springer- Verlag, 1984, Chapter 5 

23. Cayley, A., “On the Motion of Rotation of a Solid Body,” Cambridge Mathematical Journal Vol HI 
pp. 224-232, 1843 

24. Gibbs, J. Willard, and E. B. Wilson, Vector Analysis , New York, NY, Scribner, 1901 

25. Wiener, Thomas Freund, “Theoretical .Analysis of Gimballess Inertial Reference Equipment Using 

Delta-Modulated Instruments,” D. Sc. Dissertation, Massachusetts Institute of Technology, March 
1962 

26. Marandi, S. R„ and V. J. Modi, “A Preferred Coordinate System and the Associated Orientation 
Representation in Attitude Dynamics,” Acta Astronautica, Vol. 15, No. 1 1, pp. 833—843, 1987 

27. V athsal, S. "Spacecraft Attitude Determination Using a second-Order nonlinear Filter,” Journal of 
Guidance, Control, and Dynamics , Vol. 10, No. 6, pp. 559-566, 1987 

28. Magnus, W.. "On the Exponential Solution of Differential Equations for a Linear Operator." 
Communications on Pure and Applied Mathematics, Vol. 7, pp. 649-673, 1954 

-9. Park, K. C.. and Chiou, J. C., “A Discrete Momentum-Conserving Explicit Algorithm for Rigid Body 
Dynamics Analysis,” International Journal for Numerical Methods in Engineering Vol 36 dd 
1071-1073,1993 ’ 

30. Chiou, J. C., Y. W. Jan, and S. D. Wu, “Family of Constraint-Preserving Integrators for Solving 
Quaternion Equations, Journal of Guidance, Control, and Dynamics, Vol. 24, No. I, pp. 72—78, 2001 



